#include "Servo.h"


#if SERVO_ENABLE == 1
Servo_t servo = {
    .start = Servo_Start,
    .stop = Servo_Stop,
    .writeValue = Servo_WriteValue,
    .readValue = Servo_ReadValue,
    .writeAngle = Servo_WriteAngle,
    .readAngle = Servo_ReadAngle};


void Servo_Start(TIM_HandleTypeDef* phtimx, uint32_t CH, uint16_t Angle)
{
    if(phtimx != NULL)
    servo.phtimx = phtimx;
    servo.CH = CH;
    servo.AngleRange = Angle;
    servo.ValueStart = ServoStartValue(phtimx);
    servo.ValueEnd = ServoEndValue(phtimx);
    servo.ValueMax = servo.ValueEnd - ServoAngleToValue(&servo,60);
    servo.ValueMin = servo.ValueStart + ServoAngleToValue(&servo,60);
    HAL_TIM_PWM_Start(servo.phtimx, servo.CH);

}

void Servo_Stop(void)
{
    HAL_TIM_PWM_Stop(servo.phtimx, servo.CH);
}

/**
 * @brief 舵机写值
 * @param value 范围为[0,ServoValueRange]
 * @date 2022-11-12
 */
void Servo_WriteValue(uint16_t value)
{
    value += servo.ValueStart;
    if(value <= servo.ValueMin) 
        __HAL_TIM_SetCompare(servo.phtimx, servo.CH, servo.ValueMin);
    else if(value >= servo.ValueMax)
        __HAL_TIM_SetCompare(servo.phtimx, servo.CH, servo.ValueMax);
    else 
        __HAL_TIM_SetCompare(servo.phtimx, servo.CH, value);
}

/**
 * @brief 舵机读取当前值
 * @retval uint16_t 范围为[0,ServoValueRange]
 * @date 2022-11-12
 */
uint16_t Servo_ReadValue(void)
{
	return __HAL_TIM_GetCompare(servo.phtimx, servo.CH) - servo.ValueStart;
}

/**
 * @brief 写角度
 * @param angle [0,servo.AngleRange]
 * @date 2022-11-12
 */
void Servo_WriteAngle(float angle)
{
	Servo_WriteValue((int)(ServoAngleToValue(&servo, angle) + 0.5));
}

/**
 * @brief 读角度
 * @retval float [0,ServoAngle]
 * @date 2022-11-12
 */
float Servo_ReadAngle(void)
{
	return ServoValueToAngle(&servo,servo.readValue());
}

#endif

